﻿/*****************************************************************//**
 * \file   SurgicalRobotRCM.cpp
 * \brief  该文件主要处理RCM 配置
 *
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/
#include "./include/SurgicalRobotRCM.h"
#include "./include/SurgicalRobotCommon.h"

 /**
  * \brief   SurgicalRobotRCM类的构造函数.
  *
  * \param parent 父窗体默认是null.
  * \return
  */
SurgicalRobotRCM::SurgicalRobotRCM(QWidget* parent)
	: QWidget(parent)
{
	ui.setupUi(this);
	setAttribute(Qt::WA_DeleteOnClose);//自动析构
	this->setWindowTitle(QStringLiteral("RCM配置"));
	this->setWindowIcon(QIcon("hfUOT.png"));

	/*rcm1*/

	//初始化控件
	//设置值调整步长
	ui.Robot1RCM1->setSingleStep(0.0001);
	ui.Robot1RCM2->setSingleStep(0.0001);
	ui.Robot1RCM3->setSingleStep(0.0001);
	ui.Robot1RCM1->setValue(sRobotConfigPara[RobotArm0].dRCMPosition[0]);
	ui.Robot1RCM2->setValue(sRobotConfigPara[RobotArm0].dRCMPosition[1]);
	ui.Robot1RCM3->setValue(sRobotConfigPara[RobotArm0].dRCMPosition[2]);

	//事件初始化
	connect(ui.btRobot1RCMCompute, SIGNAL(clicked()), this, SLOT(RCM1Compute()));
	connect(ui.btRobot1RCMReset, SIGNAL(clicked()), this, SLOT(RCM1Reset()));
	connect(ui.btRobot1RCMSave, SIGNAL(clicked()), this, SLOT(RCM1Save()));

	/*rcm2*/
	//初始化控件
	//设置值调整步长
	ui.Robot2RCM1->setSingleStep(0.0001);
	ui.Robot2RCM2->setSingleStep(0.0001);
	ui.Robot2RCM3->setSingleStep(0.0001);
	ui.Robot2RCM1->setValue(sRobotConfigPara[RobotArm1].dRCMPosition[0]);
	ui.Robot2RCM2->setValue(sRobotConfigPara[RobotArm1].dRCMPosition[1]);
	ui.Robot2RCM3->setValue(sRobotConfigPara[RobotArm1].dRCMPosition[2]);

	//事件初始化
	connect(ui.btRobot2RCMCompute, SIGNAL(clicked()), this, SLOT(RCM2Compute()));
	connect(ui.btRobot2RCMReset, SIGNAL(clicked()), this, SLOT(RCM2Reset()));
	connect(ui.btRobot2RCMSave, SIGNAL(clicked()), this, SLOT(RCM2Save()));

	/*rcm3*/
	//初始化控件
	//设置值调整步长
	ui.Robot3RCM1->setSingleStep(0.0001);
	ui.Robot3RCM2->setSingleStep(0.0001);
	ui.Robot3RCM3->setSingleStep(0.0001);
	ui.Robot3RCM1->setValue(sRobotConfigPara[RobotArm2].dRCMPosition[0]);
	ui.Robot3RCM2->setValue(sRobotConfigPara[RobotArm2].dRCMPosition[1]);
	ui.Robot3RCM3->setValue(sRobotConfigPara[RobotArm2].dRCMPosition[2]);

	//事件初始化
	connect(ui.btRobot3RCMCompute, SIGNAL(clicked()), this, SLOT(RCM3Compute()));
	connect(ui.btRobot3RCMReset, SIGNAL(clicked()), this, SLOT(RCM3Reset()));
	connect(ui.btRobot3RCMSave, SIGNAL(clicked()), this, SLOT(RCM3Save()));

	/*rcm4*/
	//初始化控件
	//设置值调整步长
	ui.Robot4RCM1->setSingleStep(0.0001);
	ui.Robot4RCM2->setSingleStep(0.0001);
	ui.Robot4RCM3->setSingleStep(0.0001);
	ui.Robot4RCM1->setValue(sRobotConfigPara[RobotArm3].dRCMPosition[0]);
	ui.Robot4RCM2->setValue(sRobotConfigPara[RobotArm3].dRCMPosition[1]);
	ui.Robot4RCM3->setValue(sRobotConfigPara[RobotArm3].dRCMPosition[2]);

	//事件初始化
	connect(ui.btRobot4RCMCompute, SIGNAL(clicked()), this, SLOT(RCM4Compute()));
	connect(ui.btRobot4RCMReset, SIGNAL(clicked()), this, SLOT(RCM4Reset()));
	connect(ui.btRobot4RCMSave, SIGNAL(clicked()), this, SLOT(RCM4Save()));
}

/**
 * \brief  SurgicalRobotRCM类的析构函数 .
 *
 */
SurgicalRobotRCM::~SurgicalRobotRCM()
{

}

/*************************************** 机械臂1的RCM点 ***************************************/

/**
 * \brief   机械臂1的RCM点 测算.
 *
 */
void SurgicalRobotRCM::RCM1Compute()
{
#if 0
	for (int i = 0; i < 4; i++)
	{
		//if (sRobotConfigPara[i].iRobotToolType == TOOLTYPE_CAMERA)
		{
			//g_RobotControl[i].GetRobotJointStatus();
			sRobotConfigPara[i].dRCMPosition[0] = sRobotRunData[i].sRobotDy.dToolPx - 0.0115 * sRobotRunData[i].sMotorDevice.matrix4dToolEnd(0, 0);
			sRobotConfigPara[i].dRCMPosition[1] = sRobotRunData[i].sRobotDy.dToolPy - 0.0115 * sRobotRunData[i].sMotorDevice.matrix4dToolEnd(0, 1);
			sRobotConfigPara[i].dRCMPosition[2] = sRobotRunData[i].sRobotDy.dToolPz - 0.0115 * sRobotRunData[i].sMotorDevice.matrix4dToolEnd(0, 2);
			ui.Robot1RCM1->setValue(sRobotConfigPara[i].dRCMPosition[0]);
			ui.Robot1RCM2->setValue(sRobotConfigPara[i].dRCMPosition[1]);
			ui.Robot1RCM3->setValue(sRobotConfigPara[i].dRCMPosition[2]);
		}
	}
#endif

	sRobotConfigPara[0].dRCMPosition[0] = sRobotRunData[0].sRobotDy.dToolPx - 0.0115 * sRobotRunData[0].sMotorDevice.m4dToolEnd(0, 0);
	sRobotConfigPara[0].dRCMPosition[1] = sRobotRunData[0].sRobotDy.dToolPy - 0.0115 * sRobotRunData[0].sMotorDevice.m4dToolEnd(0, 1);
	sRobotConfigPara[0].dRCMPosition[2] = sRobotRunData[0].sRobotDy.dToolPz - 0.0115 * sRobotRunData[0].sMotorDevice.m4dToolEnd(0, 2);
	ui.Robot1RCM1->setValue(sRobotConfigPara[0].dRCMPosition[0]);
	ui.Robot1RCM2->setValue(sRobotConfigPara[0].dRCMPosition[1]);
	ui.Robot1RCM3->setValue(sRobotConfigPara[0].dRCMPosition[2]);

	return;
}

/**
 * \brief  机械臂1的RCM点 重置 .
 *
 */
void SurgicalRobotRCM::RCM1Reset()
{
#if 0
	for (int i = 0; i < 4; i++)
	{
		//if (sRobotConfigPara[i].iRobotToolType == TOOLTYPE_CAMERA)
		{
			sRobotConfigPara[i].dRCMPosition[0] = 0.01;
			sRobotConfigPara[i].dRCMPosition[1] = 0.01;
			sRobotConfigPara[i].dRCMPosition[2] = 0.01;
			ui.Robot1RCM1->setValue(sRobotConfigPara[i].dRCMPosition[0]);
			ui.Robot1RCM2->setValue(sRobotConfigPara[i].dRCMPosition[1]);
			ui.Robot1RCM3->setValue(sRobotConfigPara[i].dRCMPosition[2]);
		}
	}
#endif

	sRobotConfigPara[0].dRCMPosition[0] = 0.01;
	sRobotConfigPara[0].dRCMPosition[1] = 0.01;
	sRobotConfigPara[0].dRCMPosition[2] = 0.01;
	ui.Robot1RCM1->setValue(sRobotConfigPara[0].dRCMPosition[0]);
	ui.Robot1RCM2->setValue(sRobotConfigPara[0].dRCMPosition[1]);
	ui.Robot1RCM3->setValue(sRobotConfigPara[0].dRCMPosition[2]);

	return;
}

/**
 * \brief   机械臂1的RCM点 保存.
 *
 */
void SurgicalRobotRCM::RCM1Save()
{
#if 0
	for (int i = 0; i < 4; i++)
	{
		//if (sRobotConfigPara[i].iRobotToolType == TOOLTYPE_CAMERA)
		{
			sRobotConfigPara[i].dRCMPosition[0] = ui.Robot1RCM1->value();
			sRobotConfigPara[i].dRCMPosition[1] = ui.Robot1RCM2->value();
			sRobotConfigPara[i].dRCMPosition[2] = ui.Robot1RCM3->value();
		}
	}
#endif

	sRobotConfigPara[0].dRCMPosition[0] = ui.Robot1RCM1->value();
	sRobotConfigPara[0].dRCMPosition[1] = ui.Robot1RCM2->value();
	sRobotConfigPara[0].dRCMPosition[2] = ui.Robot1RCM3->value();

	SRCommunicat->SaveRobotConfig();

	return;
}

/*************************************** 机械臂2的RCM点 ***************************************/
/**
 * \brief   机械臂2的RCM点 测算.
 *
 */
void SurgicalRobotRCM::RCM2Compute()
{
#if 0
	for (int i = 0; i < 4; i++)
	{
		//通过RCM点测算，可以在QT界面读出，持电凝器械的机械臂的手术器械末端位置
		//if (sRobotConfigPara[i].iRobotToolType == TOOLTYPE_BoplarHook)
		{
			sRobotConfigPara[i].dRCMPosition[0] = sRobotRunData[i].sRobotDy.dToolPx;
			sRobotConfigPara[i].dRCMPosition[1] = sRobotRunData[i].sRobotDy.dToolPy;
			sRobotConfigPara[i].dRCMPosition[2] = sRobotRunData[i].sRobotDy.dToolPz;
			ui.Robot2RCM1->setValue(sRobotConfigPara[i].dRCMPosition[0]);
			ui.Robot2RCM2->setValue(sRobotConfigPara[i].dRCMPosition[1]);
			ui.Robot2RCM3->setValue(sRobotConfigPara[i].dRCMPosition[2]);
		}
	}
#endif

	sRobotConfigPara[1].dRCMPosition[0] = sRobotRunData[1].sRobotDy.dToolPx;
	sRobotConfigPara[1].dRCMPosition[1] = sRobotRunData[1].sRobotDy.dToolPy;
	sRobotConfigPara[1].dRCMPosition[2] = sRobotRunData[1].sRobotDy.dToolPz;
	ui.Robot2RCM1->setValue(sRobotConfigPara[1].dRCMPosition[0]);
	ui.Robot2RCM2->setValue(sRobotConfigPara[1].dRCMPosition[1]);
	ui.Robot2RCM3->setValue(sRobotConfigPara[1].dRCMPosition[2]);

	return;
}

/**
 * \brief   机械臂2的RCM点 重置.
 *
 */
void SurgicalRobotRCM::RCM2Reset()
{
#if 0
	for (int i = 0; i < 4; i++)
	{
		//通过RCM点测算，可以在QT界面读出，持电凝器械的机械臂的手术器械末端位置
		//if (sRobotConfigPara[i].iRobotToolType == TOOLTYPE_BoplarHook)
		{
			sRobotConfigPara[i].dRCMPosition[0] = 0.01;
			sRobotConfigPara[i].dRCMPosition[1] = 0.01;
			sRobotConfigPara[i].dRCMPosition[2] = 0.01;
			ui.Robot2RCM1->setValue(sRobotConfigPara[i].dRCMPosition[0]);
			ui.Robot2RCM2->setValue(sRobotConfigPara[i].dRCMPosition[1]);
			ui.Robot2RCM3->setValue(sRobotConfigPara[i].dRCMPosition[2]);
		}
	}
#endif

	sRobotConfigPara[1].dRCMPosition[0] = 0.01;
	sRobotConfigPara[1].dRCMPosition[1] = 0.01;
	sRobotConfigPara[1].dRCMPosition[2] = 0.01;
	ui.Robot2RCM1->setValue(sRobotConfigPara[1].dRCMPosition[0]);
	ui.Robot2RCM2->setValue(sRobotConfigPara[1].dRCMPosition[1]);
	ui.Robot2RCM3->setValue(sRobotConfigPara[1].dRCMPosition[2]);

	return;
}

/**
 * \brief  机械臂2的RCM点 保存 .
 *
 */
void SurgicalRobotRCM::RCM2Save()
{
#if 0
	for (int i = 0; i < 4; i++)
	{
		//通过RCM点测算，可以在QT界面读出，持电凝器械的机械臂的手术器械末端位置
		if (sRobotConfigPara[i].iRobotToolType == TOOLTYPE_BoplarHook)
		{
			sRobotConfigPara[i].dRCMPosition[0] = ui.Robot2RCM1->value();
			sRobotConfigPara[i].dRCMPosition[1] = ui.Robot2RCM2->value();
			sRobotConfigPara[i].dRCMPosition[2] = ui.Robot2RCM3->value();
		}
	}
#endif

	sRobotConfigPara[1].dRCMPosition[0] = ui.Robot2RCM1->value();
	sRobotConfigPara[1].dRCMPosition[1] = ui.Robot2RCM2->value();
	sRobotConfigPara[1].dRCMPosition[2] = ui.Robot2RCM3->value();

	SRCommunicat->SaveRobotConfig();

	return;
}


/*************************************** 机械臂3的RCM点 ***************************************/
/**
 * \brief   机械臂3的RCM点 测算.
 *
 */
void SurgicalRobotRCM::RCM3Compute()
{
#if 0
	for (int i = 0; i < 4; i++)
	{
		//if (sRobotConfigPara[i].iRobotToolType == TOOLTYPE_CAMERA)
		{
			//g_RobotControl[i].GetRobotJointStatus();
			sRobotConfigPara[i].dRCMPosition[0] = sRobotRunData[i].sRobotDy.dToolPx;
			sRobotConfigPara[i].dRCMPosition[1] = sRobotRunData[i].sRobotDy.dToolPy;
			sRobotConfigPara[i].dRCMPosition[2] = sRobotRunData[i].sRobotDy.dToolPz;
			ui.Robot3RCM1->setValue(sRobotConfigPara[i].dRCMPosition[0]);
			ui.Robot3RCM2->setValue(sRobotConfigPara[i].dRCMPosition[1]);
			ui.Robot3RCM3->setValue(sRobotConfigPara[i].dRCMPosition[2]);
		}
	}
#endif

	sRobotConfigPara[2].dRCMPosition[0] = sRobotRunData[2].sRobotDy.dToolPx;
	sRobotConfigPara[2].dRCMPosition[1] = sRobotRunData[2].sRobotDy.dToolPy;
	sRobotConfigPara[2].dRCMPosition[2] = sRobotRunData[2].sRobotDy.dToolPz;
	ui.Robot3RCM1->setValue(sRobotConfigPara[2].dRCMPosition[0]);
	ui.Robot3RCM2->setValue(sRobotConfigPara[2].dRCMPosition[1]);
	ui.Robot3RCM3->setValue(sRobotConfigPara[2].dRCMPosition[2]);

	return;
}

/**
 * \brief   机械臂3的RCM点 重置.
 *
 */
void SurgicalRobotRCM::RCM3Reset()
{

	sRobotConfigPara[2].dRCMPosition[0] = 0.01;
	sRobotConfigPara[2].dRCMPosition[1] = 0.01;
	sRobotConfigPara[2].dRCMPosition[2] = 0.01;
	ui.Robot3RCM1->setValue(sRobotConfigPara[2].dRCMPosition[0]);
	ui.Robot3RCM2->setValue(sRobotConfigPara[2].dRCMPosition[1]);
	ui.Robot3RCM3->setValue(sRobotConfigPara[2].dRCMPosition[2]);

	return;
}

/**
 * \brief   机械臂3的RCM点 保存.
 *
 */
void SurgicalRobotRCM::RCM3Save()
{
	sRobotConfigPara[2].dRCMPosition[0] = ui.Robot3RCM1->value();
	sRobotConfigPara[2].dRCMPosition[1] = ui.Robot3RCM2->value();
	sRobotConfigPara[2].dRCMPosition[2] = ui.Robot3RCM3->value();

	SRCommunicat->SaveRobotConfig();

	return;
}

/*************************************** 机械臂4的RCM点 ***************************************/
/**
 * \brief   机械臂4的RCM点 测算.
 *
 */
void SurgicalRobotRCM::RCM4Compute()
{
#if 0
	for (int i = 0; i < 4; i++)
	{
		//if (sRobotConfigPara[i].iRobotToolType == TOOLTYPE_CAMERA)
		{
			//g_RobotControl[i].GetRobotJointStatus();
			sRobotConfigPara[3].dRCMPosition[0] = sRobotRunData[i].sRobotDy.dToolPx;
			sRobotConfigPara[3].dRCMPosition[1] = sRobotRunData[i].sRobotDy.dToolPy;
			sRobotConfigPara[3].dRCMPosition[2] = sRobotRunData[i].sRobotDy.dToolPz;
			ui.Robot4RCM1->setValue(sRobotConfigPara[3].dRCMPosition[0]);
			ui.Robot4RCM2->setValue(sRobotConfigPara[3].dRCMPosition[1]);
			ui.Robot4RCM3->setValue(sRobotConfigPara[3].dRCMPosition[2]);
		}

	}
#endif

	sRobotConfigPara[3].dRCMPosition[0] = sRobotRunData[3].sRobotDy.dToolPx;
	sRobotConfigPara[3].dRCMPosition[1] = sRobotRunData[3].sRobotDy.dToolPy;
	sRobotConfigPara[3].dRCMPosition[2] = sRobotRunData[3].sRobotDy.dToolPz;
	ui.Robot4RCM1->setValue(sRobotConfigPara[3].dRCMPosition[0]);
	ui.Robot4RCM2->setValue(sRobotConfigPara[3].dRCMPosition[1]);
	ui.Robot4RCM3->setValue(sRobotConfigPara[3].dRCMPosition[2]);

	return;
}

/**
 * \brief   机械臂4的RCM点 重置.
 *
 */
void SurgicalRobotRCM::RCM4Reset()
{
	sRobotConfigPara[3].dRCMPosition[0] = 0.01;
	sRobotConfigPara[3].dRCMPosition[1] = 0.01;
	sRobotConfigPara[3].dRCMPosition[2] = 0.01;
	ui.Robot4RCM1->setValue(sRobotConfigPara[3].dRCMPosition[0]);
	ui.Robot4RCM2->setValue(sRobotConfigPara[3].dRCMPosition[1]);
	ui.Robot4RCM3->setValue(sRobotConfigPara[3].dRCMPosition[2]);

	return;
}

/**
 * \brief   机械臂4的RCM点 保存.
 *
 */
void SurgicalRobotRCM::RCM4Save()
{
	sRobotConfigPara[3].dRCMPosition[0] = ui.Robot4RCM1->value();
	sRobotConfigPara[3].dRCMPosition[1] = ui.Robot4RCM2->value();
	sRobotConfigPara[3].dRCMPosition[2] = ui.Robot4RCM3->value();

	SRCommunicat->SaveRobotConfig();

	return;
}
